% Christian Hubicki, adapted from code by Devin Koepl

classdef ForceControlModelClass < handle
    
    properties (Constant)
        %------------------------------------------------------------------
        
        % Simulation parameters
        
        % Body mass
        m   = 32;  % 32 
        
        % Moments of inertia.  The reflected leg inertia is Il / rl^2.
        Ib  = 1;
        Ih  = 1;
        Il  = 0.00286; % kg m^2   % previous value, 1
        
        % Robot stiffnesses
        kh  = 0; %1e3;
        kl  = 3e3;
        
        % Robot transmission
        G   = 50; %20
        rl  = 0.05; %0.75; %0.05 % 0.20
        
        % Gravitational acceleration
        g   = 9.81;
        
        % Preferred zero force leg length
        r0 = 0.9;
        
        %------------------------------------------------------------------
        
        % Index variables.
        
        % CoM position indices
        xci     = 1;
        dxci    = 2;
        yci     = 3;
        dyci    = 4;
        
        % Angle (theta) indices
        obi     = 5;
        dobi    = 6;
        ohi     = 7;
        dohi    = 8;
        oli     = 9;
        doli    = 10;
        
        % Toe position indices
        xti     = 12;
        yti     = 13;
        
        % Force error integral index
        fii     = 11;
        
        last_flight_index = 11;
        
        % Event indices
        LO      = 1;
        TD      = 2;
        fall    = 3;
        apex    = 4;
        
        %------------------------------------------------------------------
    end
    
    properties
        % Event variables
        te;
        xe;
        ie;
        
        % TD position
        xtd;
        ytd;
        
        % Ground properties
        kx;
        ky;
        bx;
        by;
        
        % State cell arrays
        options;
        state_fun;
        
        stance_dynamics;
        
        gnd;
        
        controller;
        
        dxtdyt;
    end
    
    methods (Static)
        
        function leg_length = R(x)
            if ( size(x, 1) == ForceControlModelClass.last_flight_index )
                % In flight so return zero force leg length (rz).
                leg_length = ForceControlModelClass.RZ(x);
            else
                % In stance so return distance from toe to CoM.
                leg_length = sqrt( ( x(ForceControlModelClass.xci, :) - x(ForceControlModelClass.xti, :) ).^2 ...
                    + ( x(ForceControlModelClass.yci, :) - x(ForceControlModelClass.yti, :) ).^2 );
            end
        end
        
        function leg_angle = PSI(x)
            if ( size(x, 1) == ForceControlModelClass.last_flight_index )
                % In flight so return zero force leg angle.
                leg_angle = x(ForceControlModelClass.ohi, :);
            else
                % If the leg is shorter than 1mm, assume that it is aligned
                % with the hip.
                if ( ForceControlModelClass.R(x) < 1e-3 )
                    leg_angle = x(ForceControlModelClass.ohi, :);
                else
                    % In stance so return angle to toe mass vector.
                    leg_angle = atan2( x(ForceControlModelClass.yci, :) - x(ForceControlModelClass.yti, :), ...
                        x(ForceControlModelClass.xci, :) - x(ForceControlModelClass.xti, :) );
                end
            end
        end
        
        function zero_force_leg_length = RZ(x)
            zero_force_leg_length = ForceControlModelClass.rl ...
                * x(ForceControlModelClass.oli, :) / ForceControlModelClass.G;
            
            %             rlcurr = ForceControlModelClass.rl
            %             olicurr = x(ForceControlModelClass.oli, :)
            %             gcurr = ForceControlModelClass.G
            
        end
        
    end
    
    methods
        
        function obj = ForceControlModelClass(x0, ground_function, controller)
            obj.te = 0;
            obj.xe = x0;
            obj.ie = 1;
            
            obj.options = { odeset( 'Events', @obj.flight_events, 'refine', 100, 'reltol', 1e-13 ),...
                odeset( 'Events', @obj.stance_events, 'refine', 100, 'reltol', 1e-13 ) };
            obj.state_fun = {@obj.flight_dynamics, obj.stance_dynamics};
            
            obj.gnd = GroundClass(ground_function);
            
            obj.controller = controller;
        end
        
        function [t, x] = run(obj)
            switch ( obj.ie )
                case obj.LO
                    % Flight phase.
                    fprintf('Flight!\n');
                    
                    % Notify the controller of liftoff.
                    obj.controller.lo( obj.te, obj.xe' );
                    
                    [t, x, obj.te, obj.xe, obj.ie] = ode113(@obj.flight_dynamics, ...
                        [obj.te, 30], obj.xe(1 : obj.last_flight_index), obj.options{obj.ie});
                    
                    rz  = obj.RZ( x' )';
                    psi = obj.PSI( x' )';
                    x   = [ x, x(:, obj.xci) - rz .* cos(psi), ...
                        x(:, obj.yci) - rz .* sin(psi) ];
                    
                    obj.te = obj.te(end);
                    obj.ie = obj.ie(end);
                    obj.xe = obj.xe(end, :);
                    
                    rz  = obj.RZ( obj.xe' );
                    psi = obj.PSI( obj.xe' );
                    
                    obj.xe   = [ obj.xe, obj.xe(obj.xci) - rz .* cos(psi), ...
                        obj.xe(obj.yci) - rz .* sin(psi) ];
                    
                case obj.TD
                    % Stance phase.  Choose the appropriate stance dynamics.
                    fprintf('Stance!\n');
                    
                    obj.dxtdyt = [ 0; 0 ];
                    
                    % Notify the controller of touchdown.
                    obj.controller.td( obj.te, obj.xe' );
                    
                    obj.xtd = obj.xe( obj.xti );
                    obj.ytd = obj.xe( obj.yti );
                    
                    obj.kx = obj.gnd.kx( obj.xtd );
                    obj.ky = obj.gnd.ky( obj.xtd );
                    obj.bx = obj.gnd.bx( obj.xtd );
                    obj.by = obj.gnd.by( obj.xtd );
                    
                    obj.gnd.new_fp(0, obj.te, obj.xtd, obj.ytd, 0, 0);
                    
                    if ( ( isinf(obj.kx) || isinf(obj.bx) ) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.rigid_ground;
                    elseif ( (obj.kx == 0) && (obj.bx > 0) && (obj.bx < inf) ...
                            && (obj.ky == 0) && (obj.bx > 0) && (obj.by < inf) )
                        obj.stance_dynamics = @obj.dissipative_ground;
                    elseif ( (obj.kx < inf) && (obj.bx > 0) && (obj.bx < inf) ...
                            && (obj.ky < inf) && (obj.bx > 0) && (obj.by < inf) )
                        obj.stance_dynamics = @obj.compliant_ground;
                    elseif ( (obj.bx == 0) && (obj.by == 0) )
                        %                         springy = 1;
                        obj.stance_dynamics = @obj.springy_ground;
                    elseif ( ( isinf(obj.kx) || isinf(obj.bx) ) && (obj.by > 0) )
                        obj.stance_dynamics = @obj.xrigid_ycompliant_ground;
                    elseif ( (obj.bx > 0) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.xcompliant_yrigid_ground;
                    elseif ( ( isinf(obj.kx) || isinf(obj.bx) ) && (obj.by == 0) )
                        obj.stance_dynamics = @obj.xrigid_yspringy_ground;
                    elseif ( ( obj.bx == 0 ) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.xspringy_ycompliant_ground;
                    elseif ( (obj.bx > 0) && (obj.by == 0) )
                        obj.stance_dynamics = @obj.xcompliant_yspringy_ground;
                    elseif ( (obj.bx == 0) && (obj.by > 0) )
                        obj.stance_dynamics = @obj.xspringy_ycompliant_ground;
                    end
                    
                    [t, x, obj.te, obj.xe, obj.ie] = ode113(obj.stance_dynamics, ...
                        [obj.te, 30], obj.xe, obj.options{obj.ie});
                    
                    %                     obj.te
                    
                    obj.te = obj.te(end);
                    obj.ie = obj.ie(end);
                    obj.xe = obj.xe(end, :);
                    
                    obj.gnd.set_lo(obj.te, obj.xe(ForceControlModelClass.xti), ...
                        obj.xe(ForceControlModelClass.yti));
                    
                    %                     figure;
                    %                     yd = obj.controller.slip_model.state(t - t(1));
                    %                     plot( t, [ x(:, obj.ohi), yd(:, SlipModelClass.oi) ] );
                    
                otherwise
                    % The model already fell, so do nothing.
                    t = obj.te;
                    x = obj.xe;
            end
            
        end
        
        %------------------------------------------------------------------
        
        % Flight
        
        function xdot = flight_dynamics(obj, t, x)
            u = obj.controller.flight(t, x);
            
            xdot = [x(obj.dxci)                         ;
                0                                   ;
                x(obj.dyci)                         ;
                -obj.g                              ;
                x(obj.dobi)                         ;
                -obj.G * u(ControllerClass.tauhi) / obj.Ib      ;
                x(obj.dohi)                         ;
                obj.G * u(ControllerClass.tauhi) / obj.Ih       ;
                x(obj.doli)                         ;
                obj.G * u(ControllerClass.tauli) / obj.Il       ;
                0                                   ];
        end
        
        function [value, isterminal, direction] = flight_events(obj, t, x)
            rz      = obj.RZ(x);
            psi     = obj.PSI(x);
            
            xt      = x(obj.xci) - rz * cos(psi);
            yt      = x(obj.yci) - rz * sin(psi);
            
            if ( x( obj.dyci ) > 0 )
                TD_event = 1;
            else
                TD_event =  yt - obj.gnd.ground_height(t, xt, 0, inf);
            end
            
            %             value       = [1 ; TD_event ; x(obj.yci) - obj.gnd.ground_height(t, x(obj.xci), 0, inf) ];     % detect height = 0
            %             isterminal  = [0 ; 1 ; 1 ];   % stop the integration
            %             direction   = [0 ; -1 ; -1 ];   % negative direction
            
            value       = [1 ; TD_event ; x(obj.yci) - obj.gnd.ground_height(t, x(obj.xci), 0, inf); x(obj.dyci)];     % detect height = 0
            isterminal  = [0 ; 1 ; 1 ; 1];   % stop the integration
            direction   = [0 ; -1 ; -1 ; -1];   % negative direction
            
            
        end
        
        %------------------------------------------------------------------
        
        % Stance
        
        % Modified by Christian Hubicki
        function xdot = rigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            % INCLUDED TO ALLOW FOR INTEGRAL CONTROLLERS
            if(strcmp(obj.controller.controller_type, 'force_integral'))
                Fs = ForceControlModelClass.kl * ( ForceControlModelClass.RZ(x) - ForceControlModelClass.R(x) );
                F_target = interp1(obj.controller.time_profile, obj.controller.force_profile, t - obj.controller.td_time);
                
                if(isnan(F_target))
                    F_target = 0;
                end
                
                dfi = F_target - Fs;
            else
                dfi = 0;
            end
            
            
            % Toe velocity
            obj.dxtdyt = [ 0; 0 ];
            
            u = obj.controller.stance( t, x );
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                - r * obj.kl * cos(psi) / obj.m;
                
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                - obj.g;
                x(obj.dobi);
                0;
                x(obj.dohi);
                0;
                x(obj.doli);
                u(ControllerClass.tauli) / obj.Il ...
                - obj.rl^2 * obj.kl * x(obj.oli) / obj.Il / obj.G^2 ...
                + obj.rl * obj.kl * r / obj.Il / obj.G;
                dfi;
                0;
                0                                             ];
            
            
        end
        
        
        function xdot = dissipative_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            % INCLUDED TO ALLOW FOR INTEGRAL CONTROLLERS
            if(strcmp(obj.controller.controller_type, 'force_integral'))
                Fs = ForceControlModelClass.kl * ( ForceControlModelClass.RZ(x) - ForceControlModelClass.R(x) );
                F_target = interp1(obj.controller.time_profile, obj.controller.force_profile, t - obj.controller.td_time);
                
                if(isnan(F_target))
                    F_target = 0;
                end
                
                
                dfi = F_target - Fs;
            else
                dfi = 0;
            end
            
            
            %% Hubicki Damping Equations 1.0
            obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.bx ...
                + obj.kl * r * cos(psi) / obj.bx;
                - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.bx ...
                + obj.kl * r * sin(psi) / obj.bx];
            
            u = obj.controller.stance(t, x, obj.dxtdyt);
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                - r * obj.kl * cos(psi) / obj.m;
                
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                - obj.g;
                x(obj.dobi);
                0;
                x(obj.dohi);
                0;
                x(obj.doli);
                u(ControllerClass.tauli) / obj.Il ...
                - obj.rl^2 * obj.kl * x(obj.oli) / obj.Il / obj.G^2 ...
                + obj.rl * obj.kl * r / obj.Il / obj.G;
                dfi;
                obj.dxtdyt];
            
        end
        
        
        
        
        function xdot = compliant_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            
            %% Hubicki Damping Equations 1.0
            %             obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.bx ...
            %                             + obj.kl * r * cos(psi) / obj.bx;
            %                            - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.bx ...
            %                             + obj.kl * r * sin(psi) / obj.bx];
            
            obj.dxtdyt = [ obj.kx * obj.xtd / obj.bx...
                - obj.kx * x(obj.xti) / obj.bx...
                - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                + obj.kl * r * cos(psi) / obj.bx...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                + obj.kh * psi * cos(psi) / obj.bx / r;
                obj.ky * obj.ytd / obj.by...
                - obj.ky * x(obj.yti) / obj.by...
                - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.by / obj.G...
                + obj.kl * r * sin(psi) / obj.by...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                + obj.kh * psi * cos(psi) / obj.by / r                              ];
            
            u = obj.controller.stance(t, x, obj.dxtdyt);
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                - r * obj.kl * cos(psi) / obj.m;
                
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                - obj.g;
                x(obj.dobi);
                0;
                x(obj.dohi);
                0;
                x(obj.doli);
                u(ControllerClass.tauli) / obj.Il ...
                - obj.rl^2 * obj.kl * x(obj.oli) / obj.Il / obj.G^2 ...
                + obj.rl * obj.kl * r / obj.Il / obj.G;
                obj.dxtdyt                                               ];
            
            %             xdot = [x(obj.dxci);
            %                 obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
            %                 - r * obj.kl * cos(psi) / obj.m...
            %                 + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
            %                 - obj.kh * psi * sin(psi) / obj.m / r;
            %                 x(obj.dyci);
            %                 obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
            %                 - obj.kl * r * sin(psi) / obj.m...
            %                 + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
            %                 - obj.kh * psi * cos(psi) / obj.m / r...
            %                 - obj.g;
            %                 x(obj.dobi);
            %                 - obj.G * u(ControllerClass.tauhi) / obj.Ib;
            %                 x(obj.dohi);
            %                 obj.kh * psi / obj.Ih...
            %                 - obj.kh * x(obj.ohi) / obj.Ih...
            %                 + obj.G * u(ControllerClass.tauhi) / obj.Ih;
            %                 x(obj.doli);
            %                 - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
            %                 + r * obj.kl * obj.rl / obj.Il...
            %                 + obj.G * u(ControllerClass.tauli) / obj.Il;
            %                 obj.dxtdyt                                               ];
        end
        
        function xdot = springy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            rdot = ( x(obj.dxci) - obj.dxtdyt(1) ) * cos(psi) ...
                + ( x(obj.dyci) - obj.dxtdyt(2) ) * sin(psi);
            psidot = ( x(obj.dyci) - obj.dxtdyt(2) ) * cos(psi) / r...
                - ( x(obj.dxci) - obj.dxtdyt(1) ) * sin(psi) / r;
            
            %% Hubicki Code 1.0
            obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.kx ...
                + obj.kl * r * cos(psi) / obj.kx;
                - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.kx ...
                + obj.kl * r * sin(psi) / obj.kx];
            
            
            %% Trimmed Devin Code
            %             obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.doli) * cos(psi) / obj.kx / obj.G...
            %                 + obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.kx / obj.G...
            %                 + obj.kl * rdot * cos(psi) / obj.kx...
            %                 - obj.kl * r * sin(psi) / obj.kx;
            %                 - obj.kl * obj.rl * x(obj.doli) * sin(psi) / obj.ky / obj.G ...
            %                 - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.ky / obj.G ...
            %                 + obj.kl * rdot * sin(psi) / obj.ky ...
            %                 + obj.kl * r * cos(psi) / obj.ky                   ];
            
            %% Devin code
            %             obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.doli) * cos(psi) / obj.kx / obj.G...
            %                 + obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.kx / obj.G...
            %                 + obj.kl * rdot * cos(psi) / obj.kx...
            %                 - obj.kl * r * sin(psi) / obj.kx...
            %                 + obj.kh * x(obj.dohi) * r * sin(psi) / obj.kx / r^2 ...
            %                 - obj.kh * x(obj.ohi) * r * cos(psi) / obj.kx / r^2 ...
            %                 - obj.kh * x(obj.ohi) * rdot * sin(psi) / obj.kx / r^2 ...
            %                 - obj.kh * psidot * r * sin(psi) / obj.kx / r^2 ...
            %                 + obj.kh * psi * r * cos(psi) / obj.kx / r^2 ...
            %                 + obj.kh * psi * rdot * sin(psi) / obj.kx / r^2;
            %                 - obj.kl * obj.rl * x(obj.doli) * sin(psi) / obj.ky / obj.G ...
            %                 - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.ky / obj.G ...
            %                 + obj.kl * rdot * sin(psi) / obj.ky ...
            %                 + obj.kl * r * cos(psi) / obj.ky ...
            %                 - obj.kh * x(obj.dohi) * r * cos(psi) / obj.ky / r^2 ...
            %                 + obj.kh * x(obj.ohi) * r * sin(psi) / obj.ky / r^2 ...
            %                 + obj.kh * x(obj.ohi) * rdot * cos(psi) / obj.ky / r^2 ...
            %                 + obj.kh * psidot * r * cos(psi) / obj.ky / r^2 ...
            %                 - obj.kh * psi * r * sin(psi) / obj.ky / r^2 ...
            %                 - obj.kh * psi * rdot * cos(psi) / obj.ky / r^2                   ];
            
            u = obj.controller.stance(t, x, obj.dxtdyt);
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                - r * obj.kl * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                obj.dxtdyt                                                      ];
        end
        
        function xdot = xrigid_ycompliant_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                0;
                obj.ky * obj.ytd / obj.by...
                - obj.ky * x(obj.yti) / obj.by...
                - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.by...
                + obj.kl * r * sin(psi) / obj.by...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                + obj.kh * psi * cos(psi) / obj.by / r                              ];
        end
        
        function xdot = xcompliant_yrigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                obj.kx * obj.xtd / obj.bx...
                - obj.kx * x(obj.xti) / obj.bx...
                - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                + obj.kl * r * cos(psi) / obj.bx...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                + obj.kh * psi * cos(psi) / obj.bx / r;
                0                                                                 ];
        end
        
        function xdot = xrigid_yspringy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                0;
                0;                                                      ];
        end
        
        function xdot = xspring_yrigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                0;
                0;                                                      ];
        end
        
        function xdot = xcompliant_yspringy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                obj.kx * obj.xtd / obj.bx...
                - obj.kx * x(obj.xti) / obj.bx...
                - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                + obj.kl * r * cos(psi) / obj.bx...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                + obj.kh * psi * cos(psi) / obj.bx / r;
                0                                                                     ];
        end
        
        function xdot = xspringy_ycompliant_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                - r * obj.kL * cos(psi) / obj.m...
                + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                - obj.kh * psi * sin(psi) / obj.m / r;
                x(obj.dyci);
                obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                - obj.kl * r * sin(psi) / obj.m...
                + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                - obj.kh * psi * cos(psi) / obj.m / r...
                - obj.g;
                x(obj.dobi);
                - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                x(obj.dohi);
                obj.kh * psi / obj.Ih...
                - obj.kh * x(obj.ohi) / obj.Ih...
                + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                x(obj.doli);
                - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                + r * obj.kl * obj.rl / obj.Il...
                + obj.G * u(ControllerClass.tauli) / obj.Il;
                0;
                obj.ky * obj.ytd / obj.by...
                - obj.ky * x(obj.yti) / obj.by...
                - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.by...
                + obj.kl * r * sin(psi) / obj.by...
                - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                + obj.kh * psi * cos(psi) / obj.by / r                              ];
        end
        
        function [value, isterminal, direction] = stance_events(obj, t, x)
            r           = obj.R(x);
            rz          = obj.RZ(x);
            
            if ( x( obj.dyci ) < 0 )
                LO_event = 1;
            else
                LO_event =  r - rz;
            end
            
            value       = [ LO_event ; 1 ; x(obj.yci) - obj.gnd.ground_height(t, x(obj.xci), 0, inf) ];
            isterminal  = [1 ; 0 ; 1 ];
            direction   = [1 ; 0 ; 0 ];
        end
        
        %------------------------------------------------------------------
        
        % Helpers
        
        %         function toe = springy_ground_toe_position(obj, x)
        %             r       = obj.R(x);
        %             psi     = obj.PSI(x);
        %
        %             toe     = [ obj.xtd...
        %                     - obj.kl * obj.rl .* x(obj.oli, :) .* cos(psi) / obj.kx / obj.G...
        %                     + obj.kl .* r .* cos(psi) / obj.kx...
        %                     - obj.kh .* x(obj.ohi) .* cos(psi) / obj.kx...
        %                     + obj.kh .* psi .* cos(psi) / obj.kx / r;
        %                 obj.ytd...
        %                     - obj.kl * obj.rl .* x(obj.oli) .* sin(psi) / obj.ky / obj.G...
        %                     + obj.kl .* r .* sin(psi) / obj.ky...
        %                     - obj.kh .* x(obj.ohi) .* cos(psi) / obj.ky / r...
        %                     + obj.kh .* psi .* cos(psi) / obj.ky / r                         ];
        %         end
        
    end
    
end